#include "Components/ComponentCreatureModel.h"
#include "Components/ComponentCreature.h"
#include "Components/ComponentBody.h"
#include "Components/ComponentLocomotion.h"

#include "GameSingleton.h"
#include "GameRegistry.hpp"
#include "GameThreadPool.hpp"
#include "GameRandom.hpp"


namespace PocketSurvival
{
    void ComponentCreatureModel::update(float dt)
    {
        if (lookRandomOrder)
        {
            Matrix matrix = componentCreature->componentBody->getMatrix();
            Vector3 v = Vector3::Normalize(m_randomLookPoint - getEyePosition());
            if (GameSingleton::gameRandom->getFloat(0.0f, 1.0f) < 0.25f * dt || Vector3::Dot(matrix.getForward(), v) < 0.2f)
            {
                float s = GameSingleton::gameRandom->getFloat(-5.0f, 5.0f);
                float s2 = GameSingleton::gameRandom->getFloat(-1.0f, 1.0f);
                float s3 = GameSingleton::gameRandom->getFloat(3.0f, 8.0f);
                m_randomLookPoint = getEyePosition() + s3 * matrix.getForward() + s2 * matrix.getUp() + s * matrix.getRight();
            }
            lookAtOrder = std::make_shared<Vector3>(m_randomLookPoint);
        }
        if (lookAtOrder != nullptr)
        {
            Vector3 forward = componentCreature->componentBody->getMatrix().getForward();
            Vector3 v2 = *(lookAtOrder) - getEyePosition();
            float x = Vector2::Angle(Vector2(forward.posX, forward.posZ), Vector2(v2.posX, v2.posZ));
            float y = MathUtils::Asin(0.99f * Vector3::Normalize(v2).posY);
            componentCreature->componentLocomotion->setLookOrder(Vector2(x, y) - componentCreature->componentLocomotion->lookAngles);
        }

        eyePositionClear();
        eyeRotationClear();
        lookRandomOrder = false;
        lookAtOrder = nullptr;
    }

    void ComponentCreatureModel::eyePositionClear()
    {
        std::lock_guard<SpinLock> lk(m_eyeLock);
        eyePosition = nullptr;
    }
    void ComponentCreatureModel::eyeRotationClear()
    {
        std::lock_guard<SpinLock> lk(m_eyeLock);
        eyeRotation = nullptr;
    }

    Vector3 ComponentCreatureModel::getEyePosition()
    {
        std::lock_guard<SpinLock> lk(m_eyeLock);
        if(eyePosition == nullptr)
        {
            eyePosition = calculateEyePosition();
        }
        return *eyePosition;
    }
    Quaternion ComponentCreatureModel::getEyeRotation()
    {
        std::lock_guard<SpinLock> lk(m_eyeLock);
        if(eyeRotation == nullptr)
        {
            eyeRotation = calculateEyeRotation();
        }
        return *eyeRotation;
    }
    std::shared_ptr<Vector3> ComponentCreatureModel::calculateEyePosition()
    {
        Vector3 position;
        componentCreature->componentBody->getPosition(position);

        Matrix matrix = componentCreature->componentBody->getMatrix();
        return std::make_shared<Vector3>(position + matrix.getUp() * 0.95f * componentCreature->componentBody->boxSize.posY + matrix.getForward() * 0.45f * position.posZ);
    }
    std::shared_ptr<Quaternion> ComponentCreatureModel::calculateEyeRotation()
    {
        Quaternion rotation;
        componentCreature->componentBody->getRotation(rotation);

        Quaternion quaternion;
        Quaternion::CreateFromYawPitchRoll(0.0f - componentCreature->componentLocomotion->lookAngles.posX, componentCreature->componentLocomotion->lookAngles.posY, 0.0f, quaternion);
        return std::make_shared<Quaternion>(rotation * quaternion);
    }

    static const std::string componentName = "CreatureModel";
    const std::string &ComponentCreatureModel::getName() const
    {
        return componentName;
    }
    const std::string &ComponentCreatureModel::GetName()
    {
        return componentName;
    }
    void ComponentCreatureModel::load(const nlohmann::json &jsonData)
    {
        componentCreature = GameSingleton::gameRegistry->get<ComponentCreature*>(entity);
    }
    void ComponentCreatureModel::save(nlohmann::json &jsonData) {}
    void ComponentCreatureModel::saveToMsgJson(nlohmann::json &jsonData) {}

} // namespace PocketSurvival
